Toward Asymptotically-Optimal Inspection Planning via Efficient Near-Optimal Graph Search

Inspection planning, the task of planning motions that allow a robot to inspect a set of points of interest, has applications in domains such as industrial, field, and medical robotics. Inspection planning can be computationally challenging, as the search space over motion plans that inspect the points of interest grows exponentially with the number of inspected points. We propose a novel method, Incremental Random Inspection-roadmap Search (IRIS), that computes inspection plans whose length and set of inspected points asymptotically converge to those of an optimal inspection plan. IRIS incrementally densifies a motion planning roadmap using sampling-based algorithms, and performs efficient near-optimal graph search over the resulting roadmap as it is generated. We demonstrate IRIS's efficacy on a simulated planar 5DOF manipulator inspection task and on a medical endoscopic inspection task for a continuum parallel surgical robot in anatomy segmented from patient CT data. We show that IRIS computes higher-quality inspection paths orders of magnitudes faster than a prior state-of-the-art method.Comment: RSS 201

Stochastic Extended LQR for Optimization-Based Motion Planning Under Uncertainty

We introduce a novel optimization-based motion planner, Stochastic Extended LQR (SELQR), which computes a trajectory and associated linear control policy with the objective of minimizing the expected value of a user-defined cost function. SELQR applies to robotic systems that have stochastic non-linear dynamics with motion uncertainty modeled by Gaussian distributions that can be state- and control-dependent. In each iteration, SELQR uses a combination of forward and backward value iteration to estimate the cost-to-come and the cost-to-go for each state along a trajectory. SELQR then locally optimizes each state along the trajectory at each iteration to minimize the expected total cost, which results in smoothed states that are used for dynamics linearization and cost function quadratization. SELQR progressively improves the approximation of the expected total cost, resulting in higher quality plans. For applications with imperfect sensing, we extend SELQR to plan in the robot's belief space. We show that our iterative approach achieves fast and reliable convergence to high-quality plans in multiple simulated scenarios involving a car-like robot, a quadrotor, and a medical steerable needle performing a liver biopsy procedure

Scalable Multicore Motion Planning Using Lock-Free Concurrency

We present PRRT (Parallel RRT) and PRRT* (Parallel RRT*), sampling-based methods for feasible and optimal motion planning designed for modern multicore CPUs. We parallelize RRT and RRT* such that all threads concurrently build a single motion planning tree. Parallelization in this manner requires that data structures, such as the nearest neighbor search tree and the motion planning tree, are safely shared across multiple threads. Rather than rely on traditional locks which can result in slowdowns due to lock contention, we introduce algorithms based on lock-free concurrency using atomic operations. We further improve scalability by using partition-based sampling (which shrinks each core’s working data set to improve cache efficiency) and parallel work-saving (in reducing the number of rewiring steps performed in PRRT*). Because PRRT and PRRT* are CPU-based, they can be directly integrated with existing libraries. We demonstrate that PRRT and PRRT* scale well as core counts increase, in some cases exhibiting superlinear speedup, for scenarios such as the Alpha Puzzle and Cubicles scenarios and the Aldebaran Nao robot performing a 2-handed task

Toward certifiable optimal motion planning for medical steerable needles

Medical steerable needles can follow 3D curvilinear trajectories to avoid anatomical obstacles and reach clinically significant targets inside the human body. Automating steerable needle procedures can enable physicians and patients to harness the full potential of steerable needles by maximally leveraging their steerability to safely and accurately reach targets for medical procedures such as biopsies. For the automation of medical procedures to be clinically accepted, it is critical from a patient care, safety, and regulatory perspective to certify the correctness and effectiveness of the planning algorithms involved in procedure automation. In this paper, we take an important step toward creating a certifiable optimal planner for steerable needles. We present an efficient, resolution-complete motion planner for steerable needles based on a novel adaptation of multi-resolution planning. This is the first motion planner for steerable needles that guarantees to compute in finite time an obstacle-avoiding plan (or notify the user that no such plan exists), under clinically appropriate assumptions. Based on this planner, we then develop the first resolution-optimal motion planner for steerable needles that further provides theoretical guarantees on the quality of the computed motion plan, that is, global optimality, in finite time. Compared to state-of-the-art steerable needle motion planners, we demonstrate with clinically realistic simulations that our planners not only provide theoretical guarantees but also have higher success rates, have lower computation times, and result in higher quality plans

A Dataset of Anatomical Environments for Medical Robots: Modeling Respiratory Deformation

Anatomical models of a medical robot's environment can significantly help guide design and development of a new robotic system. These models can be used for benchmarking motion planning algorithms, evaluating controllers, optimizing mechanical design choices, simulating procedures, and even as resources for data generation. Currently, the time-consuming task of generating these environments is repeatedly performed by individual research groups and rarely shared broadly. This not only leads to redundant efforts, but also makes it challenging to compare systems and algorithms accurately. In this work, we present a collection of clinically-relevant anatomical environments for medical robots operating in the lungs. Since anatomical deformation is a fundamental challenge for medical robots operating in the lungs, we describe a way to model respiratory deformation in these environments using patient-derived data. We share the environments and deformation data publicly by adding them to the Medical Robotics Anatomical Dataset (Med-RAD), our public dataset of anatomical environments for medical robots

Asymptotically optimal inspection planning via efficient near-optimal search on sampled roadmaps

Inspection planning, the task of planning motions for a robot that enable it to inspect a set of points of interest, has applications in domains such as industrial, field, and medical robotics. Inspection planning can be computationally challenging, as the search space over motion plans grows exponentially with the number of points of interest to inspect. We propose a novel method, Incremental Random Inspection-roadmap Search (IRIS), that computes inspection plans whose length and set of successfully inspected points asymptotically converge to those of an optimal inspection plan. IRIS incrementally densifies a motion-planning roadmap using a sampling-based algorithm and performs efficient near-optimal graph search over the resulting roadmap as it is generated. We prove the resulting algorithm is asymptotically optimal under very general assumptions about the robot and the environment. We demonstrate IRIS’s efficacy on a simulated inspection task with a planar five DOF manipulator, on a simulated bridge inspection task with an Unmanned Aerial Vehicle (UAV), and on a medical endoscopic inspection task for a continuum parallel surgical robot in cluttered human anatomy. In all these systems IRIS computes higher-quality inspection plans orders of magnitudes faster than a prior state-of-the-art method

Asymptotically Optimal Motion Planning for Learned Tasks Using Time-Dependent Cost Maps

In unstructured environments in people’s homes and workspaces, robots executing a task may need to avoid obstacles while satisfying task motion constraints, e.g., keeping a plate of food level to avoid spills or properly orienting a finger to push a button. We introduce a sampling-based method for computing motion plans that are collision-free and minimize a cost metric that encodes task motion constraints. Our time-dependent cost metric, learned from a set of demonstrations, encodes features of a task’s motion that are consistent across the demonstrations and, hence, are likely required to successfully execute the task. Our sampling-based motion planner uses the learned cost metric to compute plans that simultaneously avoid obstacles and satisfy task constraints. The motion planner is asymptotically optimal and minimizes the Mahalanobis distance between the planned trajectory and the distribution of demonstrations in a feature space parameterized by the locations of task-relevant objects. The motion planner also leverages the distribution of the demonstrations to significantly reduce plan computation time. We demonstrate the method’s effectiveness and speed using a small humanoid robot performing tasks requiring both obstacle avoidance and satisfaction of learned task constraints

High-Frequency Replanning Under Uncertainty Using Parallel Sampling-Based Motion Planning

As sampling-based motion planners become faster, they can be re-executed more frequently by a robot during task execution to react to uncertainty in robot motion, obstacle motion, sensing noise, and uncertainty in the robot’s kinematic model. We investigate and analyze high-frequency replanning (HFR), where, during each period, fast sampling-based motion planners are executed in parallel as the robot simultaneously executes the first action of the best motion plan from the previous period. We consider discrete-time systems with stochastic nonlinear (but linearizable) dynamics and observation models with noise drawn from zero mean Gaussian distributions. The objective is to maximize the probability of success (i.e., avoid collision with obstacles and reach the goal) or to minimize path length subject to a lower bound on the probability of success. We show that, as parallel computation power increases, HFR offers asymptotic optimality for these objectives during each period for goal-oriented problems. We then demonstrate the effectiveness of HFR for holonomic and nonholonomic robots including car-like vehicles and steerable medical needles